#include <hardware.h>
#include <lcd.h>

extern volatile unsigned char full_messages;
extern volatile unsigned char lcd_present;

unsigned char start_condition = START;

unsigned char twi_write(unsigned char *msg, unsigned char msg_length)
{
unsigned char r_val = 1;

//twi_putchar(0, 0, (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(1<<TWIE), start_condition);

TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);

while (!(TWCR & (1<<TWINT)));
    
if ((TWSR & 0xF8) != start_condition)
    {
    return TWSR;
    }

start_condition = R_START;

//twi_putchar(msg[0], 0, (1<<TWINT)|(1<<TWEN)|(1<<TWIE), MT_SLA_ACK);

TWDR = msg[0];
TWCR = (1<<TWINT)|(1<<TWEN);

while (!(TWCR & (1<<TWINT)));

if ((TWSR & 0xF8) != MT_SLA_ACK)
    {
    if ((TWSR & 0xF8) == MT_SLA_NACK)
        {
        if (full_messages == 1)
            {
            if (lcd_present == 1)
                {
                lcd_putsf("TWI No Slave");
                }

            strf2buffer("TWI No Slave\r");
            }
        }
        
    return TWSR;
    };

while (r_val < msg_length) 
    {
//    twi_putchar(msg[r_val], 0, (1<<TWINT)|(1<<TWEN)|(1<<TWIE), MT_DATA_ACK);

    TWDR = msg[r_val];
    TWCR = (1<<TWINT)|(1<<TWEN);

    while (!(TWCR & (1<<TWINT)));

    if ((TWSR & 0xF8) != MT_DATA_ACK)
        {
       return TWSR;
        }

    r_val++;
    };

return r_val;
}

unsigned char twi_read(unsigned char *msg, unsigned char slave_r, unsigned char msg_length)
{
unsigned char r_val = 1;

//twi_putchar(0, 0, (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(1<<TWIE), start_condition);

TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);

while (!(TWCR & (1<<TWINT)));
    
if ((TWSR & 0xF8) != start_condition)
    {
    return TWSR;
    }

start_condition = R_START;

//twi_putchar(slave_r, 0, (1<<TWINT)|(1<<TWEN)|(1<<TWIE), MR_SLA_ACK);

TWDR = slave_r;
TWCR = (1<<TWINT)|(1<<TWEN);

while (!(TWCR & (1<<TWINT)));

if ((TWSR & 0xF8) != MR_SLA_ACK)
    {
    return TWSR;
    }

while (r_val < msg_length) 
    {
//    twi_putchar(0, 1, (1<<TWINT)|(1<<TWEA)|(1<<TWEN)|(1<<TWIE), MR_DATA_ACK);

//    msg[r_val - 1] = twi_getchar();
    
    TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);

    while (!(TWCR & (1<<TWINT)));

    msg[r_val - 1] = TWDR;

    if ((TWSR & 0xF8) != MR_DATA_ACK)
        {
       return TWSR;
        }
    
    r_val++;
    };
   
//twi_putchar(0, 1, (1<<TWINT)|(1<<TWEN)|(1<<TWIE), MR_DATA_NACK);

//msg[r_val - 1] = twi_getchar();

TWCR = (1<<TWINT)|(1<<TWEN);

while (!(TWCR & (1<<TWINT)));

msg[r_val - 1] = TWDR;

if ((TWSR & 0xF8) != MR_DATA_NACK)
    {
   return TWSR;
    }

return r_val;
}

void twi_stop()
{
//twi_putchar(0, 0, (1<<TWINT)|(1<<TWEN)|(1<<TWSTO), 0);

TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);

start_condition = START;
}

void send_twi_msg(unsigned char *msg, unsigned char msg_length)
{
unsigned char result;

result = twi_write(msg, msg_length);

if (full_messages == 1 & result != msg_length)
    {
    if (lcd_present == 1)
        {
        lcd_putsf("TWI:");
        }

    strf2buffer("TWI:");
    display_value(result);
    strf2buffer("\r");
    }
}